#include "Parser.h"


Parser*
Parser::factory(const char* _type, ...) {


	Parser* item = new Parser();


	item->init();
//	item->setOptions();


	return item;
}


Parser*
Parser::init() {
	this->_V = 0;


	// FIXME
	this->_DX = 0.00002;
	this->_DY = 0.00002;
	this->_DZ = 0.00002;
	this->_VXMAX = 0.002;
	this->_VYMAX = 0.002;
	this->_VZMAX = 0.002;
	// FIXME


	return this;
}


Parser*
Parser::setRow(char* _row) {
	this->_row = _row;


	return this;
}


char*
Parser::getRow(char* _default) {
	if (this->_row) {
		return this->_row;
	}

	return _default;
}


Parser*
Parser::clear() {
	this->_row = NULL;
	this->_G = 0;
	this->_M = 0;
	this->_T = 0;
	this->_N = 0;
	this->_S = 0;
	this->_P = 0;
	this->_X = 0;
	this->_Y = 0;
	this->_Z = 0;
	this->_I = 0;
	this->_J = 0;
	this->_F = 0;
	this->_R = 0;
	this->_Q = 0;
	this->_E = 0;
	this->_checksum = 0;


	return this;
}


Parser*
Parser::parse() {
	if (!this->_row) {
		throw "Nothing to parse";
	}


	{
		char* subrow;
		for (subrow = this->_row; subrow[0]; subrow += sizeof(char)) {
			// trim space
			while (isspace(subrow[0])) {
				subrow += sizeof(char);
			}


			printf("=> %s\n", subrow);


			void* value = NULL;
			char* next = NULL;
			switch (subrow[0]) {
				// int
				case 'G':
					value? 0: value = &this->_G;
				case 'M':
					value? 0: value = &this->_M;
				case 'T':
					value? 0: value = &this->_T;
				case '*':
					value? 0: value = &this->_checksum;
				// long
				case 'N':
					value? 0: value = &this->_N;


					subrow += sizeof(char);


					// parse long value
					*(int32_t*)value = strtol(subrow, &(next = subrow), 10);
					if (next == subrow) {
						fprintf(stderr, "parser: wrong value after: \"%s\"\n", subrow);
						return this;
					}


					subrow = next;


					break;
				// float
				case 'S':
					value? 0: value = &this->_S;
				case 'P':
					value? 0: value = &this->_P;
				case 'X':
					value? 0: value = &this->_X;
				case 'Y':
					value? 0: value = &this->_Y;
				case 'Z':
					value? 0: value = &this->_Z;
				case 'I':
					value? 0: value = &this->_I;
				case 'J':
					value? 0: value = &this->_J;
				case 'F':
					value? 0: value = &this->_F;
				case 'R':
					value? 0: value = &this->_R;
				case 'Q':
					value? 0: value = &this->_Q;
				case 'E':
					value? 0: value = &this->_E;
				case 'V': /* FIXME */
					value? 0: value = &this->_V;


					subrow += sizeof(char);


					// parse float value
					*(float*)value = strtof(subrow, &(next = subrow));
					if (next == subrow) {
						fprintf(stderr, "parser: wrong value after: \"%s\"\n", subrow);
						return this;
					}


					subrow = next;


					break;
				default:
					fprintf(stderr, "parser: wrong value after: \"%s\"\n", subrow);
					return this;
			}
		}
	}


	return this;
}


Parser*
Parser::execute() {


	float v = 0;
	switch (this->_G) {
		case 1:
			v? 0: v = this->_V;
			puts("Executing G1");
///		case 2:
///			v? 0: v = this->_V;
///			puts("Executing G1");


			{
				float x = this->_X;
				float y = this->_Y;
				float z = this->_Z;


				// get moving time
				if (!v) {
					fprintf(stderr, "parser: velocity is null\n");
					return this;
				}
				float t = sqrtf(x * x + y * y + z * z) / v;


				// get velocities for axes
				float vx = x / t;
				float vy = y / t;
				float vz = z / t;
				// decrease velocities for axes limits
				if (vx > this->_VXMAX) {
					float k = this->_VXMAX / vx;
					vx *= k;
					vy *= k;
					vz *= k;
					t *= 1 / k;
				}
				if (vy > this->_VYMAX) {
					float k = this->_VYMAX / vy;
					vx *= k;
					vy *= k;
					vz *= k;
					t *= 1 / k;
				}
				if (vz > this->_VZMAX) {
					float k = this->_VZMAX / vz;
					vx *= k;
					vy *= k;
					vz *= k;
					t *= 1 / k;
				}


				// get steps count for axes
				float nx = x / this->_DX;
				float ny = y / this->_DY;
				float nz = z / this->_DZ;


				// get timers waitings for axes
//				float dtx = t / nx;
//				float dty = t / ny;
//				float dtz = t / nz;


				// get frequencies for axes
				float fx = nx / t;
				float fy = ny / t;
				float fz = nz / t;


				printf("Setting frequencies to: %.6f, %.6f, %.6f and timer to %.6f\n", fx, fy, fz, t);


				// execute
///				tone(/* digital pin */1, /* frequency */fx, /* duration, ms */t * 1000);
///				tone(/* digital pin */2, /* frequency */fy, /* duration, ms */t * 1000);
///				tone(/* digital pin */3, /* frequency */fz, /* duration, ms */t * 1000);
///				delay(/* duration, ms */t * 1000);
///				noTone(1);
///				noTone(2);
///				noTone(3);
			}


			break;
		// other
		case 0:
		default:
			fprintf(stderr, "parser: wrong command\n");
			return this;
	}


	return this;
}
